#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <time.h>
#include <pthread.h>
#include "robot.h"
#include "buffer.h"

#define BAUD_RATE B57600
#define WRITE_BUFFER_SIZE 100

static int serial;
static pthread_mutex_t serial_mutex;
static buffer write_buff;
static pthread_t write_thread;


void *robot_write_thread(void *param);

int robot_init(char *port) {
	struct termios config;
	unsigned char full_mode[] = {128, 132};

	pthread_mutex_init(&serial_mutex, NULL);
	
	init_buffer(&write_buff, WRITE_BUFFER_SIZE);

	pthread_mutex_lock(&serial_mutex);
	serial = open(port, O_RDWR | O_NOCTTY /*| O_NDELAY*/);
	if(serial < 0) {
		return -1;
	}
	config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
	//config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
	config.c_oflag = 0;
	config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
	config.c_cflag &= ~(CSIZE | PARENB);
	config.c_cflag |= CS8;
	config.c_cc[VMIN]  = 1;
	config.c_cc[VTIME] = 0;
	if (cfsetispeed(&config, BAUD_RATE) < 0 || cfsetospeed(&config, BAUD_RATE) < 0) {
		return -2;
	}
	if(tcsetattr(serial, TCSAFLUSH, &config) < 0) {
		return -3;
	}

	write(serial, full_mode, 2);
	pthread_mutex_unlock(&serial_mutex);

	pthread_create(&write_thread, NULL, robot_write_thread, NULL);

}

void *robot_write_thread(void *param) {
	buffer_item item;
	unsigned char *array;
	int size;

	while(1) {
		buffer_remove(&write_buff, &item);
		array = (unsigned char*) item;
		size = array[0];
		array += 1;
		pthread_mutex_lock(&serial_mutex);
		write(serial, array, size);
		pthread_mutex_unlock(&serial_mutex);
	}
}
	

void robot_forward() {
	unsigned char forward[] = {5, 145, 0, 100, 0, 100};
	buffer_insert(&write_buff, (buffer_item) &forward);
}

void robot_backward() {
	unsigned char backward[] = {5, 145, 255, 156, 255, 156};
	buffer_insert(&write_buff, (buffer_item) &backward);
}

void robot_left() {
	unsigned char left[] = {5, 145, 0, 100, 255, 156};
	buffer_insert(&write_buff, (buffer_item) &left);
}

void robot_right() {
	unsigned char right[] = {5, 145, 255, 156, 0, 100};
	buffer_insert(&write_buff, (buffer_item) &right);
}

void robot_stop() {
	unsigned char stop[] = {5, 145, 0, 0, 0, 0};
	buffer_insert(&write_buff, (buffer_item) &stop);
}

void robot_bumpers(unsigned char *data, time_t *timestamp) {
	unsigned char bump[] = {142, 7};

	pthread_mutex_lock(&serial_mutex);
	write(serial, bump, 2);
	//usleep(100000);
	read(serial, data, 1);
	(*data) &= 3;
	*timestamp = time(NULL);
	pthread_mutex_unlock(&serial_mutex);
}

int robot_close() {
	return close(serial);
}
